import * as THREE from "./three.js/build/three.module.js";
import Stats from '../jsm/libs/stats.module.js';
import { OrbitControls } from '../jsm/controls/OrbitControls.js';
import { GUI } from '../jsm/libs/dat.gui.module.js';
// import URDFLoader from '../jsm/URDFLoader/URDFLoader.js';


const DB_SHOW_WORD_MESHES = false;
const DB_SHOW_AXES_HELPER = true;

const PI = Math.PI;
const DOUB_PI = PI * 2;
const HALF_PI = PI * 0.5;


async function A3(container) {

  const WANTS_DEACTIVATION = 3;
  const DISABLE_DEACTIVATION = 4;
  const DISABLE_SIMULATION  = 5;

  let lastTime = 0;
  let backgroundColor = 0x1982cc;

  let stats;
  let scene, renderer, camera;
  let textureLoader;

  let gui;
  let params = {};

  const pos = new THREE.Vector3();
  const quat = new THREE.Quaternion();
  const scale = new THREE.Vector3();

  const gravityConstant = -9.8;
  const airDensityConstant = 0.4;

  let margin = 0.05;
  let collisionConfiguration;
  let dispatcher;
  let broadphase;
  let solver;
  let softBodySolver;
  let physicsWorld;
  let worldTransform;

  const meshes = [];
	const meshMap = new WeakMap();

  if ( 'Ammo' in window === false ) {

    console.error( 'AmmoPhysics: Couldn\'t find Ammo.js' );
    return;

  }

  const AmmoLib = await Ammo(); 


  // console.log(AmmoLib);
  
  initGraphics();
  initStats();
  initGui();
  initPhysics();

  function initGraphics() {

    scene = new THREE.Scene();
    scene.background = new THREE.Color( backgroundColor );

    // 坐标系辅助
    const axesHelper = new THREE.AxesHelper(1);
    scene.add(axesHelper);

    renderer = new THREE.WebGLRenderer({ alpha: true });
    // renderer.setClearColor(0XFFFFFF, 0);
    renderer.setPixelRatio( window.devicePixelRatio );
    renderer.setSize( window.innerWidth, window.innerHeight );
    renderer.shadowMap.enabled = true;
    container.appendChild( renderer.domElement );


    camera = new THREE.PerspectiveCamera(
      45, window.innerWidth / window.innerHeight, 
      0.1, 2000
    );

    camera.position.set( 10, 30, 10 );
    const controls = new OrbitControls( camera, renderer.domElement );
    controls.update();
    camera.userData.control = controls;

    textureLoader = new THREE.TextureLoader();

    const ambientLight = new THREE.AmbientLight( 0xffffff, 0.6 );
    scene.add(ambientLight)

    const light = new THREE.DirectionalLight( 0xffffff, 0.9 );
    light.position.set( 0, 20, 20 );
    light.castShadow = true;
    const d = 10;
    light.shadow.camera.left = - d;
    light.shadow.camera.right = d;
    light.shadow.camera.top = d;
    light.shadow.camera.bottom = - d;

    light.shadow.camera.near = 2;
    light.shadow.camera.far = 50;

    light.shadow.mapSize.x = 1024;
    light.shadow.mapSize.y = 1024;



    //

    window.addEventListener( 'resize', onWindowResize );

  }

  function onWindowResize() {
    camera.aspect = window.innerWidth / window.innerHeight;
    camera.updateProjectionMatrix();

    renderer.setSize( window.innerWidth, window.innerHeight );
  }

  function initStats(){
    stats = new Stats();
    stats.domElement.style.position = 'absolute';
    stats.domElement.style.top = '0px';
    container.appendChild( stats.domElement );
  }
  //////////////////////////////////////////////////////////////////////// 控制页面相关
  // 初始化控制页面
  function initGui(){
    gui = new GUI();
  }

  function createUUID( len = 8){
    var s = [];
    var hexDigits = "0123456789abcdef";
    var from = 0;
    for(var i = 0; i < len; i++){
      from = Math.floor( Math.random() * 16 );
      s[i] = hexDigits.substring( from, from + 1 );
    }

    var uuid = s.join("");

    return uuid;
  }

  function getParamUID( key ){
    while( key in params ){ key = createUUID(); }
    return key;
  }

  function addInputNumber(root , name, callback, range=[-1, 1], defaultValue = 0, key='param' ){
    
    if( isNull(root) ) root = gui;

    key = getParamUID( key );
    params[key] = defaultValue;
    
    return root.add(params, key, range[0], range[1]).name(name).onChange(callback);
  }

  function addCheckbox(root, name, callback, defaultValue = true, key='param' ){

    if( isNull(root) ) root = gui;

    key = getParamUID( key );
    params[key] = defaultValue;

    return root.add( params, key ).name(name).onChange(callback);
  }

  function addInputButton(root, name, callback, key='param' ){

    if( isNull(root) ) root = gui;

    key = getParamUID( key );
    params[key] = callback;
    return root.add( params, key ).name(name);
  }

  function addInputColor(root, name, callback, defaultValue = 0xffffff, key='param'){

    if( isNull(root) ) root = gui;

    key = getParamUID( key );
    params[key] = defaultValue;
    return root.addColor( params, key ).name(name).onChange(callback);
  }

  function addFolder(root, name, open = false ){

    if( isNull(root) ) root = gui;

    const folder = root.addFolder( name );
    if( open ) { folder.open() };

    return folder;
  }
  ///////////////////////////////////////////////////////////////////////
  function initPhysics() {
    // Physics configuration

    collisionConfiguration = new AmmoLib.btSoftBodyRigidBodyCollisionConfiguration();
    dispatcher = new AmmoLib.btCollisionDispatcher( collisionConfiguration );
    broadphase = new AmmoLib.btDbvtBroadphase();
    solver = new AmmoLib.btSequentialImpulseConstraintSolver();
    softBodySolver = new AmmoLib.btDefaultSoftBodySolver();
    physicsWorld = new AmmoLib.btSoftRigidDynamicsWorld( dispatcher, broadphase, solver, collisionConfiguration, softBodySolver );
    physicsWorld.setGravity( new AmmoLib.btVector3( 0, gravityConstant, 0 ) );
    physicsWorld.getWorldInfo().set_m_gravity( new AmmoLib.btVector3( 0, gravityConstant, 0 ) );
    physicsWorld.getWorldInfo().set_air_density ( airDensityConstant );

    worldTransform = new AmmoLib.btTransform();

  }

  function update(){

    updateGraphics();
    updatePhysics();
    stats.update();

  }
  // 更新图形界面
  function updateGraphics(){
   
    renderer.setViewport( 0, 0,  window.innerWidth, window.innerHeight );
    renderer.render( scene, camera ); 
  
    if(camera.userData.control){
      camera.userData.control.update();
    }

  }
  // 更新物理世界
  function updatePhysics(){

    const time = performance.now();
    
    if ( lastTime > 0 ) {

      const delta = ( time - lastTime ) / 1000; // console.time( 'world.step' );
      
      physicsWorld.stepSimulation( delta, 10 ); // console.timeEnd( 'world.step' );

    }

    lastTime = time;

    for ( let i = 0, il = meshes.length; i < il; i ++ ) {
      const mesh = meshes[ i ];
      
      if(DB_SHOW_WORD_MESHES){
        console.log("物体个数：", il)
      }

      if ( (mesh.isMesh || mesh.isGroup || mesh.isURDFVisual) && meshMap.has(mesh) ){
        const body = meshMap.get( mesh );
        const motionState = body.getMotionState();
        motionState.getWorldTransform( worldTransform );
        const position = worldTransform.getOrigin();
        const quaternion = worldTransform.getRotation();
        
        mesh.position.set( position.x(), position.y(), position.z() );
        mesh.quaternion.set( quaternion.x(), quaternion.y(), quaternion.z(), quaternion.w() );

      }
    }
  }

  function updateBody( threeObject , body ){
    scene.updateMatrixWorld(true);
    const pos = new THREE.Vector3();
    const quat = new THREE.Quaternion();
    const scale = new THREE.Vector3();

    threeObject.matrixWorld.decompose(pos, quat, scale);
    // const worldPosition = new THREE.Vector3();
    // threeObject.getWorldPosition(worldPosition);

    const motionState = body.getMotionState();
    motionState.getWorldTransform( worldTransform );

    const position = worldTransform.getOrigin();
    const quaternion = worldTransform.getRotation();

    position.setValue(
      pos.x,
      pos.y,
      pos.z
    );

    quaternion.setValue( 
      quat.x, 
      quat.y, 
      quat.z,
      quat.w
    );

    worldTransform.setRotation( quaternion );

    body.setWorldTransform( worldTransform );

  }
  // 获取网格几何图形的物理形状
  function getShape( geometry ){
  
    const parameters = geometry.parameters;

    let shape = undefined;

    if (geometry.isBufferGeometry){
      const type = geometry.type;
      switch(type){
        case "BoxGeometry":
          const sx = parameters.width !== undefined ? parameters.width / 2 : 0.5;
          const sy = parameters.height !== undefined ? parameters.height / 2 : 0.5;
          const sz = parameters.depth !== undefined ? parameters.depth / 2 : 0.5;
          shape = new AmmoLib.btBoxShape( new AmmoLib.btVector3( sx, sy, sz ) );
          shape.setMargin( margin );
          break;
        case "CylinderGeometry":
          console.log(geometry);
          const height = parameters.height !== undefined ? parameters.height / 2 : 0.5;
          const radiusTop = parameters.radiusTop !== undefined ? parameters.radiusTop : 1;
          const radiusBottom = parameters.radiusBottom !== undefined ? parameters.radiusBottom : 1;

          shape = new AmmoLib.btCylinderShape( new AmmoLib.btVector3( radiusTop, height, radiusBottom ) );
          shape.setMargin( margin );
          break;
        case "BufferGeometry":
          shape = createConvexHullPhysicsShape( geometry.attributes.position.array );
          
          shape.setMargin(margin);
          break;
        default:
          console.error("不支持的网格类型:", geometry, type);
          break;
      }
    }
    else{

      console.error("不支持的几何形状:", geometry);

    }

    return shape;
  }
  // 由网格坐标生成凸包物理形状
  function createConvexHullPhysicsShape( coords , scale=1 ) {

    const shape = new AmmoLib.btConvexHullShape();
    var tempBtVec3_1 = new AmmoLib.btVector3( 0, 0, 0 );

    for ( let i = 0, il = coords.length; i < il; i += 3 ) {

      // coords[ i ]*=10;coords[ i + 1 ]*=10;coords[ i + 2 ]*=10;

      tempBtVec3_1.setValue( coords[ i ], coords[ i + 1 ], coords[ i + 2 ] );
      const lastOne = ( i >= ( il - 3 ) );
      shape.addPoint( tempBtVec3_1, lastOne );

    }

    shape.setLocalScaling(new AmmoLib.btVector3( scale, scale, scale ));

    return shape;

  }
  // 将网格数据和物理形状添加到物理世界和图形界面
  function handleMesh( mesh, mass, shape ) {

    // console.log("..Add Mesh", mesh, mass, shape)
    
    const position = mesh.position;
    const quaternion = mesh.quaternion;

    const transform = new AmmoLib.btTransform();

    transform.setIdentity();
    transform.setOrigin( new AmmoLib.btVector3( position.x, position.y, position.z ) );
    transform.setRotation( new AmmoLib.btQuaternion( quaternion.x, quaternion.y, quaternion.z, quaternion.w ) );
    
    const motionState = new AmmoLib.btDefaultMotionState( transform );
    const localInertia = new AmmoLib.btVector3( 0, 0, 0 );

    shape.calculateLocalInertia( mass, localInertia );

    const rbInfo = new AmmoLib.btRigidBodyConstructionInfo( mass, motionState, shape, localInertia );
    const body = new AmmoLib.btRigidBody( rbInfo ); 

    if ( mass > 0 ) {

      // meshes.push( mesh );
      meshMap.set( mesh, body );

      // Disable deactivation
      body.setActivationState( DISABLE_DEACTIVATION );

    }

    handleThreeObject( mesh , body);
    handlePhysicsBody( mesh, body );

    return mesh;
  }
  // 添加坐标轴辅助线
  function addAxesHelper( mesh, size ){

    if( !size ){
      if(mesh.isMesh){

        if(!mesh.geometry.boundingSphere){

          mesh.geometry.computeBoundingSphere();

        }

        var sphere = mesh.geometry.boundingSphere;
        size = sphere.radius;
      }
    }
    size = size ? size:1.0;
    const axesHelper = new THREE.AxesHelper( size );
    mesh.add(axesHelper);
  }
  // 添加到图形界面
  function handleThreeObject( threeObject, body ){
   
    meshes.push ( threeObject );

    scene.add( threeObject );

    if( DB_SHOW_AXES_HELPER ){
      addAxesHelper( threeObject , 0.5);
    }

    // 默认开启阴影
    threeObject.castShadow = true;
    threeObject.receiveShadow = true;

    if( !isNull(body) ){
      threeObject.userData.physicsBody = body;
    }
  }
  // 添加到物理世界
  function handlePhysicsBody( threeObject, body ){
    // 设置用户指针指向网格模型
    const btVecUserData = new Ammo.btVector3( 0, 0, 0 );
    btVecUserData.threeObject = threeObject;
    body.setUserPointer( btVecUserData );

    physicsWorld.addRigidBody( body );
  }
  // 添加网格
  function addMesh( mesh, mass = 0 , position, quaternion) {

    const shape = getShape( mesh.geometry );

    if(position){
      mesh.position.copy(position);
    }

    if(quaternion){
      mesh.quaternion.copy(quaternion);
    }

    if ( shape !== null ) {

      return handleMesh( mesh, mass, shape );

    }

  }
  // 移除网格
  function removeMesh( mesh ){

    for(var i = 0, il = meshes.length; i < il; i++){
      if( mesh === meshes[i] ){
        disposeMesh(mesh);
        meshes.splice(i, 1);
        meshMap.delete(mesh);
        // console.log(meshes, meshMap);
        return;
      }
    }

  }
  // 销毁网格占用内存
  function disposeMesh( mesh ){
    //todo: 释放资源
    scene.remove(mesh);
    physicsWorld.removeRigidBody(meshMap[mesh]);
  }

  // 添加地面
  function addGround( width, height, length, textureFile ){
    pos.set( 0, - height * 0.5, 0 );
    quat.set( 0, 0, 0, 1 );

    const geometry = new THREE.BoxGeometry( width, height, length, 1, 1, 1 );
    const mesh = new THREE.Mesh(geometry, new THREE.MeshPhongMaterial( { color : 0xcccccc } ));
    
    textureLoader.load( textureFile, function ( texture ) {

      texture.wrapS = THREE.RepeatWrapping;
      texture.wrapT = THREE.RepeatWrapping;
      texture.repeat.set(  width, length );
      mesh.material.map = texture;
      mesh.material.needsUpdate = true;

    } );

    mesh.position.copy(pos);
    mesh.quaternion.copy(quat);

    this.addMesh( mesh, 0 );
  }

  // 添加立方体物体
  function createParalellepiped( sx, sy, sz, mass, pos, quat, material ) {

    const threeObject = new THREE.Mesh( new THREE.BoxGeometry( sx, sy, sz, 1, 1, 1 ), material );
    const shape = getShape( threeObject.geometry );
    shape.setMargin( margin );

    return createRigidBody( threeObject, shape, mass, pos, quat );

  }

  // 复合形状
  class A3CompoundShape{
    shape = undefined;
    transform = new AmmoLib.btTransform();

    constructor(){

      this.shape = new AmmoLib.btCompoundShape();
      this.transform.setIdentity();

    }

    addShape( child, position, quaternion ){
      const transform = this.transform;
      // transform.setIdentity();

      transform.setOrigin( new AmmoLib.btVector3( position.x, position.y, position.z ) );
      transform.setRotation( new AmmoLib.btQuaternion( quaternion.x, quaternion.y, quaternion.z, quaternion.w ) );

      this.shape.addChildShape( transform, child );
    }

    getShape(){

      return this.shape;

    }
  }

  // 生成刚体并添加到物理世界
  function createRigidBody( threeObject, shape, mass, position, quaternion, inertia ) {
    
    if( isNull(position) ) {
      position = threeObject.position;
    }
    else {
      threeObject.position.copy(position);
    }
    
    if( isNull(quaternion) ){
      quaternion = threeObject.quaternion;
    }
    else{
      threeObject.quaternion.copy(quaternion);
    }

    if( isNull( inertia ) ){
      inertia = new AmmoLib.btVector3( 0, 0, 0 );
    }

    const transform = new AmmoLib.btTransform();

    transform.setIdentity();
    transform.setOrigin( new AmmoLib.btVector3( position.x, position.y, position.z ) );
    transform.setRotation( new AmmoLib.btQuaternion( quaternion.x, quaternion.y, quaternion.z, quaternion.w ) );
    
    const motionState = new AmmoLib.btDefaultMotionState( transform );

    shape.calculateLocalInertia( mass, inertia );

    const rbInfo = new AmmoLib.btRigidBodyConstructionInfo( mass, motionState, shape, inertia );
    const body = new AmmoLib.btRigidBody( rbInfo ); 

    if ( mass > 0 ) {

      // meshes.push( threeObject ); // move to handleThreeObject
      meshMap.set( threeObject, body );

      // Disable simulation
      body.setActivationState( DISABLE_SIMULATION );

    }
    threeObject.userData.physicsBody = body;
    // handleThreeObject( threeObject , body);
    handlePhysicsBody( threeObject, body );

    return threeObject;
  }

  // 获取物体世界坐标位置
  function getMatrixWorld( mesh ){
    scene.updateMatrixWorld(true);
    mesh.matrixWorld.decompose(pos, quat, scale);
    return mesh.matrixWorld;
  }

  function getWorldPosition( mesh ){
    getMatrixWorld( mesh ) ;
    return pos;
  }

  function getWorldQuaternion( mesh ){
    getMatrixWorld( mesh ) ;
    return quat;
  }
  
  function getBodyAngularVelocity( body , axis ){
    const av = body.getAngularVelocity();
    
    return axis.x * av.x() + axis.y * av.y() + axis.z * av.z();
  }
  // 铰链约束
  function addHingeConstraint( rbA, rbB, pivotInA, pivotInB, axisInA, axisInB, useReferenceFrameA = true ){

    if( isNull( pivotInA ) ){
      pivotInA = new AmmoLib.btVector3( 0, 0, 0 );
    }

    if( isNull( pivotInB ) ){
      pivotInB = new AmmoLib.btVector3( 0, 0, 0 );
    }
    
    if( isNull( axisInA ) ){
      axisInA = new AmmoLib.btVector3( 0, 1, 0 );
    }

    if( isNull( axisInB ) ){
      axisInB = axisInA;
    }
    
    const hinge = new Ammo.btHingeConstraint( rbA, rbB, pivotInA, pivotInB, axisInA, axisInB, useReferenceFrameA );

    physicsWorld.addConstraint( hinge, true );

    return hinge;
  }

  function addHingeConstraint1( rbA, rbB, pivotInA, pivotInB, axisA, axisB){

    if(bodyA.isMesh){
      bodyA = meshMap.has(bodyA)?meshMap.get(bodyA):bodyA.userData.physicsBody;
    }

    if(bodyB.isMesh){
      bodyB = meshMap.has(bodyB)?meshMap.get(bodyB):bodyB.userData.physicsBody;
    }

    pivotA = pivotA !== undefined? pivotA : new AmmoLib.btVector3(0, 0, 0);
    pivotB = pivotB !== undefined? pivotB : new AmmoLib.btVector3(0, 0, 0);

    axisA = axisA !== undefined? axisA : new AmmoLib.btVector3(0, 1, 0);
    axisB = axisB !== undefined? axisB : new AmmoLib.btVector3(0, 1, 0);


    const hinge = new Ammo.btHingeConstraint( 
      bodyA, bodyB, 
      pivotA, pivotB, 
      axisA, axisB, true 
    );

    physicsWorld.addConstraint( hinge, true );

    return hinge;
  }

  function addHingeConstraint2(rbA, rbB, frameInA, frameInB, useReferenceFrameA= true){
    if(rbA.isMesh || rbA.isGroup){
      rbA = meshMap.has(rbA)?meshMap.get(rbA):rbA.userData.physicsBody;
    }

    if(rbB.isMesh || rbB.isGroup){
      rbB = meshMap.has(rbB)?meshMap.get(rbB):rbB.userData.physicsBody;
    }

    const hinge = new Ammo.btHingeConstraint( 
      rbA, rbB, 
      frameInA, frameInB, 
      useReferenceFrameA 
    );

    physicsWorld.addConstraint( hinge, true );

    return hinge;
  }

  // 6Dof 约束
  function add6DofConstraint( bodyA, bodyB, frameInA, frameInB, useReferenceFrameA = true,
    lowerSliderLimit, hiSliderLimit, lowerAngularLimit, hiAngularLimit
  ){

    // const constraint = new Ammo.btGeneric6DofSpringConstraint  (
    //   bodyA,
    //   bodyB,
    //   frameInA,
    //   frameInB,
    //   useReferenceFrameA
    // );

    const constraint = new Ammo.btGeneric6DofConstraint  (
      bodyA,
      bodyB,
      frameInA,
      frameInB,
      useReferenceFrameA
    );

    if( lowerSliderLimit  ) { constraint.setLinearLowerLimit( lowerSliderLimit ) };
    if( hiSliderLimit     ) { constraint.setLinearUpperLimit( hiSliderLimit ) };
    if( lowerAngularLimit ) { constraint.setAngularLowerLimit( lowerAngularLimit ) };
    if( hiAngularLimit    ) { constraint.setAngularUpperLimit( hiAngularLimit ) };

    physicsWorld.addConstraint(constraint)

    return constraint;
  }

  // 转动副
  class RevoluteJoint{

    _jointType = "revolute";

    // useReferenceFrameA 
    useReferenceFrameA = true;

    effort = 10;
    lower = -DOUB_PI;
    upper = DOUB_PI;
    maxVelocity = PI;
    damping = 10;
    friction = 0.1;
 
    constructor( parent, child, name="revolute" ){

      this.parent = parent;
      this.child = child;
      this.name = name;

      this.pivotInA = new THREE.Vector3(0, 0, 0);
      this.pivotInB = new THREE.Vector3(0, 0, 0);

      this.axisInA = new THREE.Vector3(0, 1, 0);
      this.axisInB = new THREE.Vector3(0, 1, 0);

      this.hinge = undefined;
      this.angle = 0;
      this.targetVelocity = 0;

    }

    init( pivotInA, pivotInB, axisInA, axisInB ){
      if( !isNull( pivotInA ) ){
        this.pivotInA = pivotInA;
      }
      else{
        pivotInA = this.pivotInA;
      }
  
      if( !isNull( pivotInB ) ){
        this.pivotInB = pivotInB;
      }
      else{
        pivotInB = this.pivotInB;
      }
      
      if( !isNull( axisInA ) ){
        this.axisInA = axisInA;
      }
      else{
        axisInA = this.axisInA;
      }
  
      if( !isNull( axisInB ) ){
        this.axisInB = axisInB;
      }
      else{
        axisInB = this.axisInB;
      }

      const rbA = this.parent.userData.physicsBody;
      const rbB = this.child.userData.physicsBody;

      const hinge = new Ammo.btHingeConstraint( 
        rbA, rbB, 
        new Ammo.btVector3( pivotInA.x, pivotInA.y, pivotInA.z ),
        new Ammo.btVector3( pivotInB.x, pivotInB.y, pivotInB.z ),
        new Ammo.btVector3( axisInA.x, axisInA.y, axisInA.z ),
        new Ammo.btVector3( axisInB.x, axisInB.y, axisInB.z ),
        this.useReferenceFrameA 
      );

      physicsWorld.addConstraint( hinge, true );

      hinge.enableAngularMotor(true, 0, this.effort);

      this.hinge = hinge;
      this.angle = 0;

    }

    setJointVelocity( targetVelocity , impulse) {

      if( isNull(impulse) ) { impulse = this.effort; }
      if( Math.abs(targetVelocity) > this.maxVelocity ) { 
        targetVelocity = targetVelocity > 0 ? this.maxVelocity : -this.maxVelocity ; 
      }

      if( this.hinge ){
        // this.targetVelocity = targetVelocity;
        this.hinge.enableAngularMotor( true, targetVelocity, impulse );
      }

    }

    getJointVelocity(){

      const v1 = getBodyAngularVelocity( this.parent.userData.physicsBody, this.axisInA );
      const v2 = getBodyAngularVelocity( this.child.userData.physicsBody, this.axisInB );

      return ( v2 - v1 );
    }

    getAngleA(){

      if(this.axisInA.x() === 1) { return this.parent.rotation.x; }
      if(this.axisInA.y() === 1) { return this.parent.rotation.y; }
      if(this.axisInA.z() === 1) { return this.parent.rotation.z; }

    }

    getAngleB(){

      if(this.axisInB.x() === 1) { return this.child.rotation.x; }
      if(this.axisInB.y() === 1) { return this.child.rotation.y; }
      if(this.axisInB.z() === 1) { return this.child.rotation.z; }

    }

    setAngle( angle ){

      if(this.axisInB.x() === 1) { this.child.rotation.x = angle; }
      if(this.axisInB.y() === 1) { this.child.rotation.y = angle; }
      if(this.axisInB.z() === 1) { this.child.rotation.z = angle; }

      const body = this.child.userData.physicsBody;
      const motionState = body.getMotionState();
      motionState.getWorldTransform( worldTransform );
      const quaternion = worldTransform.getRotation();

      quaternion.setValue( 
        this.child.quaternion.x, 
        this.child.quaternion.y, 
        this.child.quaternion.z,
        this.child.quaternion.w
      );

      worldTransform.setRotation( quaternion );

      body.setWorldTransform( worldTransform );

      if( this.enableBrake ){
        this.hinge.enableAngularMotor( true, 0, this.effort );
      }

    }

    setJointValue( angle ){
      angle += this.getAngleA();

      if( angle > this.upper ) { angle = this.upper; }
      if( angle < this.lower ) { angle = this.lower; }
      
      this.angle = angle;
      this.target = angle;

      angle = angle % DOUB_PI;

      if( angle > HALF_PI ){
        angle = angle - DOUB_PI;
      }

      this.setAngle( angle );

    }

    getJointValue(){

      return ( this.angle );

    }

    update(){
      // this.updateJointAngle();
      
      this.hinge.enableAngularMotor(true, this.targetVelocity, this.effort);
    }

    print(){
      console.log( 
        "Name:", this.name,
        "Parent:", this.parent,
        "Child:", this.child,
        "PivotInA", this.pivotInA,
        "PivotInB", this.pivotInB,
        "AxisInA", this.axisInA,
        "AxisInB", this.axisInB,
      );
    }

    dispose(){
      physicsWorld.removeConstraint( this.hinge );
    }
  }

  return{

    AmmoLib:AmmoLib,
    RevoluteJoint,RevoluteJoint,
    A3CompoundShape,A3CompoundShape,
    update:update,
    updateBody:updateBody,
    getWorldPosition,getWorldPosition,
    getWorldQuaternion,getWorldQuaternion,
    getShape:getShape,
    addMesh:addMesh,
    createConvexHullPhysicsShape,createConvexHullPhysicsShape,
    createRigidBody:createRigidBody,
    addThreeObject:handleThreeObject,
    removeMesh:removeMesh,
    addGround:addGround,
    addHingeConstraint:addHingeConstraint,
    addHingeConstraint1, addHingeConstraint1,
    addHingeConstraint2,addHingeConstraint2,
    add6DofConstraint,add6DofConstraint,
    ////////////////////////////////////////////// GUI
    addInputNumber,addInputNumber,
    addCheckbox,addCheckbox,
    addInputButton,addInputButton,
    addInputColor,addInputColor,
    addFolder,addFolder,
    //////////////////////////////////////////////////

  }
}

function createMaterial( color , transparent=false, wireframe=false) {

  return new THREE.MeshPhongMaterial( { 
    color: color?color:createRandomColor(), 
    transparent: transparent, 
    opacity: 0.5,
    wireframe:wireframe
  } );

}

function createRandomColor() {

  return Math.floor( Math.random() * ( 1 << 24 ) );

}

function isNull( obj ){
  return ( obj === null || obj === undefined ) ;
}

function isEqual(a, b){
  const delta = PI / 360;
  return ( Math.abs(a - b) <= delta );
}


export default A3;